#include "SDFDof.h"
#include <iostream>

#include <SDFContact.h>
#include <SeparateDirectionFunction.h>
#include <FileSystem.h>
SDFDof::SDFDof() : osg::Node()
{
    m_xAxisInWorldFrame = osg::Vec3(1,0,0);
    m_yAxisInWorldFrame = osg::Vec3(0,1,0);
    m_zAxisInWorldFrame = osg::Vec3(0,0,1);

    m_axisSignal.push_back(0);
    m_axisSignal.push_back(0);
    m_axisSignal.push_back(0);
    m_axisSignal.push_back(0);
    m_axisSignal.push_back(0);
    m_axisSignal.push_back(0);

}

SDFDof::~SDFDof()
{

}

int SDFDof::getDofCount()
{
    if(m_axisSignal.size() <6)
        return 0;
    int dofCount = 0;
    for(int i = 0; i<m_axisSignal.size(); i++)
    {
        if(m_axisSignal[i] == true)
            dofCount++;
    }
    return dofCount;
}

std::vector<bool> SDFDof::getAxisSignal()
{
    return m_axisSignal;
}

void SDFDof::setSdfRigidPtrInt(int sdfRigidPtrInt)
{
    m_sdfRigidPtrInt = sdfRigidPtrInt;
}

int SDFDof::getSdfRigidPtrInt()
{
    return m_sdfRigidPtrInt;
}

void SDFDof::setXAxisInWorldFrame(osg::Vec3 &axis)
{
    m_xAxisInWorldFrame = axis;
}

void SDFDof::setYAxisInWorldFrame(osg::Vec3 &axis)
{
    m_yAxisInWorldFrame = axis;
}

void SDFDof::setZAxisInWorldFrame(osg::Vec3 &axis)
{
    m_zAxisInWorldFrame = axis;
}

osg::Vec3 SDFDof::getXAxisInWorldFrame()
{
    return m_xAxisInWorldFrame;
}

osg::Vec3 SDFDof::getYAxisInWorldFrame()
{
    return m_yAxisInWorldFrame;
}

osg::Vec3 SDFDof::getZAxisInWorldFrame()
{
    return m_zAxisInWorldFrame;
}

void SDFDof::setAxisSignal(std::vector<bool>& axisSignal)
{
    m_axisSignal = axisSignal;
}

void SDFDof::printAxisSignal()
{
    for(int i = 0 ; i < m_axisSignal.size(); i++)
    {
        std::cout<<m_axisSignal[i]<<",";
    }
    std::cout<<std::endl;
}

void SDFDof::setContacts(std::vector<osg::ref_ptr<SDFContact> > contacts)
{
    m_contacts = contacts;
}

void SDFDof::computeAxis()
{
/*    osg::Vec3 resultantForce;
    for(int i = 0; i < m_contacts.size(); i++)
    {
        resultantForce += m_contacts.at(i)->computeResultantForce();
    }
    resultantForce.normalize();
//    std::cout<<"SDFDof::computeAxis resultantForce="
//            <<resultantForce.x()<<","
//            <<resultantForce.y()<<","
//            <<resultantForce.z()<<std::endl;
*/
    std::vector<osg::Vec3> contacts;
    for(int i=0; i<m_contacts.size(); i++)
    {
        double signal = 1.0;
        if(m_sdfRigidPtrInt == m_contacts.at(i)->getSDF1()->getRigidPtrInt())
            signal = -1.0;
        for(int j = 0; j < m_contacts.at(i)->getContactPointsCount(); j++)
        {
            contacts.push_back(m_contacts.at(i)->getContactNormalByIndex(j) * signal);
        }
    }
    Cppipm::addContacts(contacts);
    Cppipm::setb(-0.05);
    Cppipm::solve();
    osg::Vec3 dir = Cppipm::getDirection();

    FileSystem::instance()->saveVec3VectorToMatlabFile("tempFiles/MatlabFile/A_1.txt",contacts);

    std::cout<<"SDFDof::computeAxis dir="
            <<dir.x()<<","
            <<dir.y()<<","
            <<dir.z()<<std::endl;


}
